#include "BezierCoordinate.h"
#include <windows.h>
#include <GL/gl.h>

CBezierCoordinate::CBezierCoordinate(int numberOfElement): m_numberOfElement(numberOfElement), m_firstTime(true){}

void CBezierCoordinate::calculMatrix(void)
{
	int numberOfPath=m_vectorOfBezierPoint.size()/4;

	for(int path=0; path<numberOfPath; path++)//for each path
	{
		for(int i=0; i<m_numberOfElement; i++)//for each element
		{
			float time=1.f/m_numberOfElement*i;

			float oneMinusTimePower3 = (1-time)*(1-time)*(1-time);
			float oneMinusTimePower2 = (1-time)*(1-time);
			float oneMinusTime = 1-time;
			float timePower2=time*time;
			float timePower3=timePower2*time;

			Vector3 point1=m_vectorOfBezierPoint[path*4];
			Vector3 point2=m_vectorOfBezierPoint[path*4+1];
			Vector3 point3=m_vectorOfBezierPoint[path*4+2];
			Vector3 point4=m_vectorOfBezierPoint[path*4+3];

			//position
			Vector3 position;
			position.x = oneMinusTimePower3*point1.x+3*time*oneMinusTimePower2*point2.x+
				3*timePower2*oneMinusTime*point3.x+timePower3*point4.x;
		
			position.y = oneMinusTimePower3*point1.y+3*time*oneMinusTimePower2*point2.y+
				3*timePower2*oneMinusTime*point3.y+timePower3*point4.y;

			position.z = oneMinusTimePower3*point1.z+3*time*oneMinusTimePower2*point2.z+
				3*timePower2*oneMinusTime*point3.z+timePower3*point4.z;

			//first derivative
			Vector3 firstDerivative;
			firstDerivative.x=3*(m_vectorOfBezierPoint[path*4+1].x-m_vectorOfBezierPoint[path*4].x)*oneMinusTimePower2+
					6*(m_vectorOfBezierPoint[path*4+2].x-m_vectorOfBezierPoint[path*4+1].x)*(time-time*time)+
					3*(m_vectorOfBezierPoint[path*4+3].x-m_vectorOfBezierPoint[path*4+2].x)*time*time;
			firstDerivative.y=3*(m_vectorOfBezierPoint[path*4+1].y-m_vectorOfBezierPoint[path*4].y)*oneMinusTimePower2+
					6*(m_vectorOfBezierPoint[path*4+2].y-m_vectorOfBezierPoint[path*4+1].y)*(time-time*time)+
					3*(m_vectorOfBezierPoint[path*4+3].y-m_vectorOfBezierPoint[path*4+2].y)*time*time;
			firstDerivative.z=3*(m_vectorOfBezierPoint[path*4+1].z-m_vectorOfBezierPoint[path*4].z)*oneMinusTimePower2+
					6*(m_vectorOfBezierPoint[path*4+2].z-m_vectorOfBezierPoint[path*4+3].z)*(time-time*time)+
					3*(m_vectorOfBezierPoint[path*4+3].z-m_vectorOfBezierPoint[path*4+2].z)*time*time;

			//second derivative
			Vector3 secondDerivative;
			secondDerivative.x=(m_vectorOfBezierPoint[path*4+1].x-m_vectorOfBezierPoint[path*4].x)*(-6+6*time)+
							(m_vectorOfBezierPoint[path*4+2].x-m_vectorOfBezierPoint[path*4+1].x)*(6-12*time)+
							(m_vectorOfBezierPoint[path*4+3].x-m_vectorOfBezierPoint[path*4+2].x)*6*time;
			secondDerivative.y=(m_vectorOfBezierPoint[path*4+1].y-m_vectorOfBezierPoint[path*4].y)*(-6+6*time)+
							(m_vectorOfBezierPoint[path*4+2].y-m_vectorOfBezierPoint[path*4+1].y)*(6-12*time)+
							(m_vectorOfBezierPoint[path*4+3].y-m_vectorOfBezierPoint[path*4+2].y)*6*time;
			secondDerivative.z=(m_vectorOfBezierPoint[path*4+1].z-m_vectorOfBezierPoint[path*4].z)*(-6+6*time)+
							(m_vectorOfBezierPoint[path*4+2].z-m_vectorOfBezierPoint[path*4+1].z)*(6-12*time)+
							(m_vectorOfBezierPoint[path*4+3].z-m_vectorOfBezierPoint[path*4+2].z)*6*time;

			float magnitudeSecondDerivative=tsg::math::length(secondDerivative);	
			if(secondDerivative.y<0)
			{
				secondDerivative.y*=-1;
			}

			if(magnitudeSecondDerivative==0)
			{
				secondDerivative.x=0;
				secondDerivative.y=0;
				secondDerivative.z=-1;
			}
			Matrix m=tsg::math::frenet(position, firstDerivative, secondDerivative);
			m_matrixRail.push_back(m);
		}
	}
}
